﻿/*
  planner.c - buffers movement commands and manages the acceleration profile plan
  Part of Grbl

  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Jens Geisler

  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "grbl.h"


static plan_block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
static uint8_t block_buffer_tail;     // 尾部有效块，待取出的队列尾Index of the block to process now
static uint8_t block_buffer_head;     // 空有效位置（待存入位置）
static uint8_t next_buffer_head;      // 下个头部有效块
static uint8_t block_buffer_planned;  // 被优化过的块索引Index of the optimally planned block

// 存储了运动规划所需的所有状态和参数，包括G代码块队列、当前块和位置、速度和加速度等
typedef struct {
  int32_t position[N_AXIS];          // 当前位置，以绝对步进值表示
                                     // 与G代码位置分开存储，用于需要多个线段运动的运动，例如弧线、 
                                     // canned cycles（ canned cycles 是一种预定义的循环运动，用于执行特定的加工任务）和反向间隙补偿。
  float previous_unit_vec[N_AXIS];   // 前一个路径线段的单位向量。单位向量用于表示路径的方向，在计算弧线等运动时非常有用。
  float previous_nominal_speed;      // 前一个路径线段的名义速度。名义速度是规划器用于计算和调度运动的参考速度。
} planner_t;
static planner_t pl;


// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
uint8_t plan_next_block_index(uint8_t block_index)
{
  block_index++;
  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
  return(block_index);
}


// Returns the index of the previous block in the ring buffer
static uint8_t plan_prev_block_index(uint8_t block_index)
{
  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
  block_index--;
  return(block_index);
}

/*                                  规划器速度定义
                                     +--------+   <- current->nominal_speed 当前加工速度
                                    /          \
         current->entry_speed ->   +            \
                                   |             + <- next->entry_speed (下个入口速度也是退出速度)
                                   +-------------+
                                      time 时间 -->

根据以下基本准则重新计算运动计划：

1. 按照可行的块在时间上的逆序排列，依次计算各个交接速度（即当前入口速度 current->entry_speed）：
  a. 确保没有交接速度超过预计算的最大交接速度限制或相邻块的名义速度。
  b. 块的入口速度不能超过从其出口速度（下一个->入口速度）反向计算出的速度，且在块的行程距离内最大允许的减速度下。
  c. 最后（或最近添加的）块从完全停止（出口速度为零）开始规划。
2. 按照时间上的顺序（正序）对每个块进行遍历，如果需要则降低交接速度值：
  a. 如果出口速度超过从其入口速度正向计算出的速度，且在块的行程距离内最大允许的加速度下。
  
当这些阶段完成后，规划器将通过所有规划块最大化速度曲线，使得每个块都在其最大允许的加速度限制下操作。 
换句话说，对于规划中的所有块，该计划是最优的，无法再进一步改进速度。如果向缓冲区添加了新的块，将根据以上准则重新计算以获得新的最优计划。

为了提高计算效率，已创建了一组规划块指针，指示计算停止点，即在正常操作下，当规划准则无法逻辑性地做出任何进一步的变化或改进时。
例如，如果规划器中一系列连续的块已被规划，并且其交接速度达到最大（或由第一个规划块界定），则添加新的块到规划缓冲区也不会改变这些块的速度曲线。
因此，我们不再需要计算它们。或者，如果从规划器中的第一个块（或最优停止计算点）开始的一系列连续块都在加速，则它们在添加新的块到规划缓冲区时也无法被改变，
因为这只会进一步提高速度直到达到最大交接速度。然而，如果规划的操作条件发生变化，例如偶尔使用的进给保持或进给速率覆盖，计算停止指针将被重置，
整个计划将根据上述通用准则重新计算。

规划器缓冲区索引映射：

-block_buffer_tail: 指向规划器缓冲区的开始位置。首先被执行或正在执行。
-block_buffer_head: 指向缓冲区中最后一个缓冲块之后的块。用于指示缓冲区是满的还是空的。如标准环形缓冲区所述，此块始终为空。
-next_buffer_head: 指向缓冲头块之后的下一个缓冲块。当等于缓冲尾指针时，表示缓冲区已满。
-block_buffer_planned: 指向最后一个最优规划块之后的缓冲块，用于正常流式操作条件下的规划优化。
 通过避免重新计算新块添加后不会改变的部分来提高效率，如上所述。
 此外，此块永远不能小于block_buffer_tail，并且在遇到plan_discard_current_block()程序例程时会向前推进以保持这一要求。

注意：由于规划器仅计算缓冲区中的内容，对于包含大量短线段的动作，如G2/3圆弧或复杂曲线，可能会显得移动缓慢。
这是因为整个缓冲区中没有足够的行程距离来加速到名义速度并在缓冲区结束时减速到完全停止，如准则所述。
如果出现这种情况并引起不便，可以尝试以下简单的解决方案：
  (1) 最大化机器的加速度。规划器将能够在相同组合距离内计算出更高的速度曲线。
  (2) 最大化每个块的线性运动距离。规划器有更多的组合距离可以使用，速度就能更快。
  (3) 最大化规划器缓冲区大小。这也将增加规划器计算的组合距离。但它也会增加规划器需要执行的计算次数以计算一个最优计划，所以必须谨慎选择。
      对于Arduino 328p的内存已经饱和，但未来的ARM版本应该会有足够的内存和速度来支持多达一百个甚至更多的前瞻块。
*/

// 到规划缓冲区中的最后一个块（即最近被添加进来的块），然后检查这个块是否已经被优化过。
// 如果没有被优化过，函数会继续执行后续的逻辑，
// 通过反向规划的方式尝试最大化可能的减速曲线，以确保运动在到达当前规划终点时能够平滑地停止。
static void planner_recalculate()
{
  // 最新的线段块索引（最近加入）。block_buffer_head 指向带加入的空白位置。
  uint8_t block_index = plan_prev_block_index(block_buffer_head);

   // 退出。如果只有一个可规划的块，无法进行任何操作。
  if (block_index == block_buffer_planned) { return; }

  // 反向传递：从缓冲区中的最后一个块开始，粗略地最大化所有可能的减速曲线，反向规划。
  // 当达到最后一个最优规划的块或尾部指针时停止规划。
  // 注意：前向传递将稍后细化并纠正反向传递，以创建最优规划。
  float entry_speed_sqr;
  plan_block_t *next;
  plan_block_t *current = &block_buffer[block_index];

  // 最后块出口速度为0，根据移动距离计算全减速运行时所允许的最大入口速度，并取最小值
  current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);

  block_index = plan_prev_block_index(block_index);
  if (block_index == block_buffer_planned) {    // 缓冲区中只有两个可规划的块。反向传递完成。
    // 如果这个块是最后一个块，则通知步进器更新其 segment 步进微段参数。因为前面优化可能会改变其出口速度
    if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
  } else { // 更多块时进行优化，知道优化到最优块索引位置
    while (block_index != block_buffer_planned) {
      next = current;
      current = &block_buffer[block_index];
      block_index = plan_prev_block_index(block_index);

      // 如果当前块是最后一个尾规划块，通知步进器更新其 segment 步进微段参数。
      if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }

      // 根据当前块的出口速度计算反向加速运行时所允许的最大入口速度，并取最小值。确保能减速到出口速度。
      if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
        // v1^2 - V2^2 = 2*a*ΔS --> v1^2 = V2^2 + 2*a*ΔS
        entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
        if (entry_speed_sqr < current->max_entry_speed_sqr) {
          current->entry_speed_sqr = entry_speed_sqr;
        } else {
          current->entry_speed_sqr = current->max_entry_speed_sqr;
        }
      }
    }
  }

  // 正向规划（时间从前向后，队列从尾到头）：从规划指针开始的加速曲线。
  // 也会扫描最优规划断点并相应地更新规划指针。
  block_index = block_buffer_planned;
  next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
  block_index = plan_next_block_index(block_buffer_planned);
  while (block_index != block_buffer_head) {
    current = next;
    next = &block_buffer[block_index];

    // 如果当前块入口速度小于下一个块入口速度，则认为处于加速过程，更新下一个块入口速度
    // 前面的轨迹都是最优的，因此将最优规划指针向前移动
    if (current->entry_speed_sqr < next->entry_speed_sqr) {
      entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
      // If true, current block is full-acceleration and we can move the planned pointer forward.
      if (entry_speed_sqr < next->entry_speed_sqr) {
        next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
        block_buffer_planned = block_index;     // 最优规划位置移动到下个
      }
    }

    // 任何设置在其最大入口速度的块也会使其之前的缓冲区中的规划达到最优。当规划被缓冲区的开始和一个最大入口速度，
    // 或者两个最大入口速度所限制时，中间的所有块逻辑上无法进一步优化。因此，我们不再需要重新计算它们。
    // 计算的入口速度已经等于最大入口速度，则认为当前轨迹块已经是最优的了
    if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; }
    block_index = plan_next_block_index( block_index );
  }
}


void plan_reset()
{
  memset(&pl, 0, sizeof(planner_t)); // Clear planner struct
  plan_reset_buffer();
}


void plan_reset_buffer()
{
  block_buffer_tail = 0;
  block_buffer_head = 0; // Empty = tail
  next_buffer_head = 1; // plan_next_block_index(block_buffer_head)
  block_buffer_planned = 0; // = block_buffer_tail;
}


void plan_discard_current_block()
{
  if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
    uint8_t block_index = plan_next_block_index( block_buffer_tail );
    // Push block_buffer_planned pointer, if encountered.
    if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; }
    block_buffer_tail = block_index;
  }
}

// 返回系统运动所使用的计划器缓冲区块的地址。 由段生成器调用。
// Returns address of planner buffer block used by system motions. Called by segment generator.
plan_block_t *plan_get_system_motion_block()
{
  return(&block_buffer[block_buffer_head]);
}

// 如果可以返回规划器第一个块的地址。被一系列主程序函数调用。
// Returns address of first planner block, if available. Called by various main program functions.
plan_block_t *plan_get_current_block()
{
  if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty
  return(&block_buffer[block_buffer_tail]);
}


float plan_get_exec_block_exit_speed_sqr()
{
  uint8_t block_index = plan_next_block_index(block_buffer_tail);
  if (block_index == block_buffer_head) { return( 0.0 ); }
  return( block_buffer[block_index].entry_speed_sqr );
}


// Returns the availability status of the block ring buffer. True, if full.
uint8_t plan_check_full_buffer()
{
  if (block_buffer_tail == next_buffer_head) { return(true); }
  return(false);
}


// 计算进给速度（加工线段的线速度）
// 如果是快速移动 G 代码，则乘以系统速率覆盖百分比 r_override
// 如果 G 代码不是禁止进给率覆盖，则乘以系统进给率覆盖百分比 f_override
// 如果计算出的进给速率大于块的最大进给速率，则将其限制为块的最大进给速率
float plan_compute_profile_nominal_speed(plan_block_t *block)
{
  float nominal_speed = block->programmed_rate;   // 块的编程进给速率
  if (block->condition & PL_COND_FLAG_RAPID_MOTION) { nominal_speed *= (0.01*sys.r_override); }
  else {
    if (!(block->condition & PL_COND_FLAG_NO_FEED_OVERRIDE)) { nominal_speed *= (0.01*sys.f_override); }
    if (nominal_speed > block->rapid_rate) { nominal_speed = block->rapid_rate; } // 轴速度限制
  }

  // 如果计算出的进给速率小于最小进给速率，则将其限制为最小进给速率
  if (nominal_speed > MINIMUM_FEED_RATE) { return(nominal_speed); }
  return(MINIMUM_FEED_RATE);
}

// 线段入口速度按当前和上个线段加工速度与转角速度的最小值
// 入口速度小于等于相邻两个段进给速度
// 入口速度小于等于最大转角速度
static void plan_compute_profile_parameters(plan_block_t *block, float nominal_speed, float prev_nominal_speed)
{
  // 最大入口速度等于相邻最小的进给速度
  if (nominal_speed > prev_nominal_speed) { block->max_entry_speed_sqr = prev_nominal_speed*prev_nominal_speed; }
  else { block->max_entry_speed_sqr = nominal_speed*nominal_speed; }

    // 入口速度小于等于转角速度
  if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) { block->max_entry_speed_sqr = block->max_junction_speed_sqr; }
}


// Re-calculates buffered motions profile parameters upon a motion-based override change.
void plan_update_velocity_profile_parameters()
{
  uint8_t block_index = block_buffer_tail;
  plan_block_t *block;
  float nominal_speed;
  float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation.
  while (block_index != block_buffer_head) {
    block = &block_buffer[block_index];
    nominal_speed = plan_compute_profile_nominal_speed(block);
    plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed);
    prev_nominal_speed = nominal_speed;
    block_index = plan_next_block_index(block_index);
  }
  pl.previous_nominal_speed = prev_nominal_speed; // Update prev nominal speed for next incoming block.
}


/* 
  添加一个线性运动到缓冲区。target[N_AXIS]是带符号的，绝对目标位置，以毫米为单位。进给率指定了运动速度。
  如果进料速率颠倒，则进料速率表示“频率”，将在1/进料速率分钟内完成操作。
  传递给规划器的所有位置数据必须根据机器位置保持规划器独立于任何坐标系统更改和偏移，这些更改和偏移由g-code解析器处理。
  注意：假设缓冲区可用。 缓冲区检查由motion_control在更高级别处理。  换句话说，缓冲头永远不等于缓冲尾。
  此外，馈电速率输入值以三种方式使用:
  如果invert_feed_rate为假，则作为正常馈电速率;
  如果invert_feed_rate为真，则作为逆时间;如果feed_rate为负，则作为seek/rapids速率(且invert_feed_rate始终为假)。  
  系统运动条件告诉规划者在始终未使用的块缓冲头中计划一个运动。 它避免了改变规划器的状态，并保留缓冲区，以确保后续的gcode动作仍然正确规划，而步进模块只指向块缓冲区头执行特殊的系统动作。   
 */
uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data)
{
  // 准备初始化新块，拷贝相关的pl_data给块执行。
  plan_block_t *block = &block_buffer[block_buffer_head];
  memset(block,0,sizeof(plan_block_t)); // Zero all block values. 所有块值归零
  block->condition = pl_data->condition;

  // 计算和储存初始的移动距离数据
  int32_t target_steps[N_AXIS], position_steps[N_AXIS];
  float unit_vec[N_AXIS], delta_mm;
  uint8_t idx;

  // 根据计划的运动类型复制位置数据。 
  if (block->condition & PL_COND_FLAG_SYSTEM_MOTION) { 
      memcpy(position_steps, sys_position, sizeof(sys_position)); 
  } else { memcpy(position_steps, pl.position, sizeof(pl.position)); }

  // 循环计算每个轴，计算目标位置的绝对步长，每个轴的步长数，并确定最大步长事件。
  // 此外，计算独立轴距离的移动和准备单位矢量计算。  
  // 注意:计算从转换的步长值的真实距离。  
  for (idx=0; idx<N_AXIS; idx++) {
    target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); // 目标相对原点绝对位置，mm 四舍五入到步数
    block->steps[idx] = labs(target_steps[idx]-position_steps[idx]);    // 计算当前轴需要移动的增量步数
    block->step_event_count = max(block->step_event_count, block->steps[idx]); // 计算每轴最大增量步长
      
    // 计算移动单位向量，并确定每个轴的移动距离
    // 轴移动毫米距离
    delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
    unit_vec[idx] = delta_mm;   // 每轴实际要移动的距离 mm

    // 这个值小于零，说明这个轴需要向与原来方向相反的方向移动。设置方向位。
    if (delta_mm < 0.0 ) { block->direction_bits |= get_direction_pin_mask(idx); }
  }

  // 计算直线移动的单位矢量和块的最大进给速率和加速度,按比例缩小，这样就不会超过相对于直线方向的单个轴的最大值。
  // 注意:这个计算假设所有的轴都是正交的(笛卡尔)，并且适用于abc轴。

  // 计算移动的模长，并计算移动的模长为 1 单位向量，也是每个轴的 cos 值
  block->millimeters = convert_delta_vector_to_unit_vector(unit_vec);
  // 移动向量上的加速度和最大线速度取三个轴中最小的那个
  block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec);
  block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec);

  // 快速移动则使用轴限制的最大速度。快速移动是指机床以最大速度从一个位置移动到另一个位置，通常用于机床的快速定位。
  if (block->condition & PL_COND_FLAG_RAPID_MOTION) { block->programmed_rate = block->rapid_rate; }
  else { 
    block->programmed_rate = pl_data->feed_rate;
    if (block->condition & PL_COND_FLAG_INVERSE_TIME) { block->programmed_rate *= block->millimeters; }
  }

  // 快缓存为空，则该线段为第一个线段，入口速度从 0 开始
  if ((block_buffer_head == block_buffer_tail) || (block->condition & PL_COND_FLAG_SYSTEM_MOTION)) {
    // 初始化块进入速度为零。假设从静止开始。 规划器稍后会更正这一点。 
    // 如果系统运动，系统运动块总是假定从静止开始并在完全停止时结束。
    block->entry_speed_sqr = 0.0;
    block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.

  } else {
    /**
     * 用向心加速度近似计算最大允许进入速度。
     * 设一个圆与先前的和当前的路径线段相切，其中结偏差定义为从结到最近的圆边缘的距离，与圆心共线。  
     * 连接两条路径的圆段表示向心加速度的路径。 
     * 根据绕圆半径的最大加速度求解最大速度，该加速度由结的偏差间接定义。  
     * 在之前的Grbl版本中，这也可以看作是路径宽度或max_jerk。
     * 这种方法实际上不会偏离路径，但作为一种稳健的方法来计算转弯速度，因为它考虑了交点角和交点速度的非线性。 
     * 注:如果结偏差值是有限的，Grbl以精确路径模式(G61)进行运动。
     * 如果结偏差值为零，Grbl将以精确停止模式(G61.1)的方式执行运动。
     * 在未来，如果想要连续模式(G64)，这里的数学是完全相同的。 
     * 机器不会一直移动到连接点，它只会沿着这里定义的圆弧。 
     * Arduino没有执行连续模式路径的CPU周期，但基于arm的微控制器肯定有。  
     * 
     * 注意:最大连接速度是一个固定值，因为机器的加速限制不能在运行过程中动态改变，也不能移动线的几何形状。
     * 当进给速度覆盖改变块的标称速度时，这必须保存在内存中，这可能改变所有块的整体最大进入速度条件。
     **/
    float junction_unit_vec[N_AXIS];
    float junction_cos_theta = 0.0;
    for (idx=0; idx<N_AXIS; idx++) {
      //计算两条线段的夹角余弦值，夹角余弦公式cosa=(x1*x2+y1*y2+z1*z2)/(s1*s2)，
      //因为两条线段是首尾相连，那么用两条线段的向量坐标计算出来的夹角其实是它的补角，
      //夹角和它补角的余弦值刚好取负值即可，所以下面计算夹角余弦的方法里多了一个负号 
      junction_cos_theta -= pl.previous_unit_vec[idx]*unit_vec[idx];
      junction_unit_vec[idx] = unit_vec[idx]-pl.previous_unit_vec[idx];     // 转接向量
    }

    // 注意:不用任何昂贵的三角函数sin()或acos()，通过半角角恒等式cos()来计算。
    // cosθ=(x1*x2+y1*y2+z1*z2)/(s1*s2)
    // sin(θ/2)=±sqrt((1-cosθ)/2) 
    // r = δ*sin(θ/2)/(1-sin(θ/2))
    // v*v = a*r
    // v = sqrt(a*r)
    // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
    if (junction_cos_theta > 0.999999) {
      // 对于0度急转弯，只需设置最小转弯速度即可。 
      block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED;
    } else {
      if (junction_cos_theta < -0.999999) {
        // 交接处是一条直线或180度。 连接速度是无限的。  
        block->max_junction_speed_sqr = SOME_LARGE_VALUE;
      } else {
        // 使用内切圆计算最大转角速度
        convert_delta_vector_to_unit_vector(junction_unit_vec);
        // 允许转接点最大加速度
        float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec);
        float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
        block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
                       (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
      }
    }
  }

  // 更新数据以执行下个 G 代码
  if (!(block->condition & PL_COND_FLAG_SYSTEM_MOTION)) {
    float nominal_speed = plan_compute_profile_nominal_speed(block);  // 加工线速度
    plan_compute_profile_parameters(block, nominal_speed, pl.previous_nominal_speed);
    
    // 更新上个路径速度、位置和单位向量
    pl.previous_nominal_speed = nominal_speed;
    memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
    memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]

    // 更新环形队列位置
    block_buffer_head = next_buffer_head;
    next_buffer_head = plan_next_block_index(block_buffer_head);

    // 重新计算和优化轨迹
    planner_recalculate();
  }
  return(PLAN_OK);
}


// Reset the planner position vectors. Called by the system abort/initialization routine.
void plan_sync_position()
{
  // TODO: For motor configurations not in the same coordinate frame as the machine position,
  // this function needs to be updated to accomodate the difference.
  uint8_t idx;
  for (idx=0; idx<N_AXIS; idx++) {
    #ifdef COREXY
      if (idx==X_AXIS) {
        pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position);
      } else if (idx==Y_AXIS) {
        pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position);
      } else {
        pl.position[idx] = sys_position[idx];
      }
    #else
      pl.position[idx] = sys_position[idx];
    #endif
  }
}


// Returns the number of available blocks are in the planner buffer.
uint8_t plan_get_block_buffer_available()
{
  if (block_buffer_head >= block_buffer_tail) { return((BLOCK_BUFFER_SIZE-1)-(block_buffer_head-block_buffer_tail)); }
  return((block_buffer_tail-block_buffer_head-1));
}


// Returns the number of active blocks are in the planner buffer.
// NOTE: Deprecated. Not used unless classic status reports are enabled in config.h
uint8_t plan_get_block_buffer_count()
{
  if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); }
  return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head));
}


// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize()
{
  // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
  st_update_plan_block_parameters();
  block_buffer_planned = block_buffer_tail;
  planner_recalculate();
}
